#ifndef RADAR_3D_TRACKING_H
#define RADAR_3D_TRACKING_H

#include <stdint.h>
#include <stdbool.h>
#include <math.h>

// 雷达点迹数据（三维极坐标）
// 距离r(m)、方位角theta(rad，水平角)、俯仰角phi(rad，垂直角)、时间戳
typedef struct {
    double r;           // 径向距离
    double theta;       // 方位角（xy平面，从x轴逆时针为正）
    double phi;         // 俯仰角（xz平面，从x轴向上为正）
    uint64_t timestamp; // 时间戳(ms)
} RadarPoint3D;

// 目标状态（三维直角坐标）
typedef struct {
    double x;   // x坐标(m)
    double y;   // y坐标(m)
    double z;   // z坐标(m)
    double vx;  // x方向速度(m/s)
    double vy;  // y方向速度(m/s)
    double vz;  // z方向速度(m/s)
} TargetState3D;

// 卡尔曼滤波参数（三维匀速度模型）
typedef struct {
    // 状态向量 [x, y, z, vx, vy, vz]^T（6维）
    double x[6];
    
    // 协方差矩阵 P (6x6)
    double P[6][6];
    
    // 状态转移矩阵 F (6x6)，基于采样时间dt
    double F[6][6];
    
    // 观测矩阵 H (3x6)，观测x, y, z
    double H[3][6];
    
    // 过程噪声协方差 Q (6x6)
    double Q[6][6];
    
    // 测量噪声协方差 R (3x3)
    double R[3][3];
} KalmanFilter3D;

// 目标航迹信息
typedef struct {
    uint32_t id;          // 航迹ID
    TargetState3D state;  // 当前状态
    KalmanFilter3D kf;    // 卡尔曼滤波器
    uint8_t life;         // 航迹寿命
    uint8_t max_life;     // 最大寿命（超过则删除）
    bool is_valid;        // 航迹是否有效
} Track3D;

// 系统配置
#define MAX_TRACKS_3D 8     // 最大同时跟踪目标数
#define INIT_LIFE_3D 3      // 航迹起始所需连续匹配次数
#define MAX_LIFE_3D 5       // 最大存活帧数
#define GATE_THRESHOLD 8.0  // 波门阈值（三维马氏距离）

#endif